/*
 * Vision.cpp
 *
 *  Created on: Nov 13, 2013
 *      Author: Josh Panka
 */

#define VISIONFILE
#include "Vision.h"
#include <string>

#include <dynamixel.h>

int motorSpeed;
int xmotor = 0;
int ymotor = 0;
string start;

CvCapture* capture = 0;
IplImage* frame=0;
//MotorController motorController;


//Motor Initialization
int xmotor_position = 1998;
int ymotor_position = 1136;

// TODO For drawing image
//IplImage* imgTracking;


Vision::Vision() {
	initMotorUSB();
	xmotor = 23;//setMotor('X');
	ymotor = 24;//setMotor('Y');
	int motorSpeed = 200;//setSpeed();

	//commenting these out to test android control

	//std::cout<<"Press any key, then ENTER to start tracking"<<std::endl;
	//std::cin>>start;

	dxl_write_word(ymotor, MOVING_SPEED, motorSpeed);
	dxl_write_word(xmotor, MOVING_SPEED, motorSpeed);

	// TODO This doesn't get executed, why?
	dxl_write_word(ymotor, GOAL_POSITION, ymotor_position);
	dxl_write_word(xmotor, GOAL_POSITION, xmotor_position);

	capture =0;
	// TODO Should be CAM(0)
	capture = cvCaptureFromCAM(0);
	if(!capture){
		printf("Capture failure\n");
	}

	frame=0;
	frame = cvQueryFrame(capture);
	if(!frame)
	{ /* Deal with error */
		std::cout << "Unable to get frame." << std::endl;
	}

//	else {
		// TODO For drawing image
//		imgTracking=cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
//		cvZero(imgTracking);

//		cvNamedWindow("Video");
//	}
}

Vision::~Vision() {
	cvDestroyAllWindows();
	cvReleaseImage(&frame);
	cvReleaseCapture(&capture);
}

int Vision::step(bool isFalling) {
	if (isFalling)
	{
		// Deal with this case

		// This shouldn't happen, as we aren't connecting the inertial data yet, but we need to return something.
		return -1;
	}
	else
	{
		int ballCurPos = 0;
		int	bodyMovement = 0;
		int c = 0;
		frame = cvQueryFrame(capture);
		if(!frame)
		{ /* Deal with error */ }

//		else {
			frame=cvCloneImage(frame);

			cvSmooth(frame, frame, CV_GAUSSIAN,3,3);

			//detectCircles(frame);
			ballCurPos = detectContours(frame);
			std::cout << "Ball Current Position " << ballCurPos << std::endl;
			// TODO For drawing image
//			DrawGridFull(frame);
//			cvShowImage("Video", frame);

			/**
			 * the following 6 lines of code will return a motion index to MUL8.cpp, which uses that as a parameter to tell MotorController.cpp how to move the body
			 */
			//if(ballCurPos == 5){ //why must is equal 5??

			// FINAL RETURN VALUE

			bodyMovement = getBodyMovement(); //retrieve body movement code (0-5)
			//}


			cvReleaseImage(&frame);


			c = cvWaitKey(5);
			if((char)c==27 ) { return -1; }

			return bodyMovement;
//		}

	}
}


//Find the position of the ball in the web cam grid (positions 1-9)
int Vision::findPos(IplImage* img, float xpos, float ypos){
	//Track left-side of View
	if(xpos <= 213 && ypos <=160){
		//left top
		return 1;
	}
	else if(xpos <= 213 && ypos >160 && ypos <=320){
		//left middle
		return 4;
	}
	else if(xpos <= 213 && ypos >320){
		//left bottom
		return 7;
	}
	//Track middle of view
	else if(xpos > 213 && xpos<=426 && ypos <=160){
		//center top
		return 2;
	}
	else if(xpos > 213 && xpos<=426 && ypos >160 && ypos <=320){
		//center middle
		return 5;
	}
	else if(xpos > 213 && xpos<=426 && ypos >320){
		//center bottom
		return 8;
	}
	//Track right-side of view
	else if(xpos>426 && ypos <=160){
		//right top
		return 3;
	}
	else if(xpos>426 && ypos >160 && ypos <=320){
		//right middle
		return 6;
	}
	else if(xpos>426 && ypos >320){
		//right bottom
		return 9;
	}
	return 0;
}

//Move the head motor the the grid position found in the findPos method
void Vision::moveHeadMotors(int gridPos){
	//	std::cout << xmotor_position << "\t" << ymotor_position << std::endl;
	//	// These outputs should be the same as the line above, but they aren't
	//	std::cout << vision_dynamixel.dxl_read_word(xmotor, PRESENT_POSITION) << "\t" << vision_dynamixel.dxl_read_word(ymotor, PRESENT_POSITION) << std::endl;
	int dMotor = 20;

	// TODO Optimize if it works
	switch(gridPos){


	// Right Bottom
	case 1:
		xmotor_position -= dMotor;
		ymotor_position -= dMotor;
		std::cout << "top left\t";
		break;


		// Center Bottom
	case 2:
		ymotor_position -= dMotor;
		std::cout << "top middle\t";
		break;

		// Left Bottom
	case 3:
		xmotor_position += dMotor;
		ymotor_position -= dMotor;
		std::cout << "top right\t";
		break;

		// Right Middle
	case 4:
		xmotor_position -= dMotor;
		std::cout << "left Middle\t";
		break;

		// Center Middle
	case 5:
		std::cout << "center middle\t";
		break;

		// Left Middle
	case 6:
		xmotor_position += dMotor;
		std::cout << "right middle\t";
		break;

		// Right Top
	case 7:
		xmotor_position -= dMotor;
		ymotor_position += dMotor;
		std::cout << "left bottom\t";
		break;

		// Center Top
	case 8:
		ymotor_position += dMotor;
		std::cout << "Center bottom\t";
		break;

		// Left Top
	case 9:
		xmotor_position += dMotor;
		ymotor_position += dMotor;
		std::cout << "right bottom\t";
		break;
	default:
		break;
	}
	if (ymotor_position < 356)	{ ymotor_position = 356; }
	if (ymotor_position > 1916) { ymotor_position = 1916; }
	if (xmotor_position < 635)	{ xmotor_position = 635;  }
	if (xmotor_position > 3360)	{ xmotor_position = 3360; }

	dxl_write_word(xmotor, GOAL_POSITION, xmotor_position);
	dxl_write_word(ymotor, GOAL_POSITION, ymotor_position);

	return;
}

//move the body depending on where the head is located after it is in grid pos 5
int Vision::getBodyMovement(){
	int moveToLocation = -1;

	if(xmotor_position>=1885 && xmotor_position<=2115 && ymotor_position<500){
		//printf("\nKick");
		moveToLocation = KICK;
		std::cout << "Kicking" << std::endl;
	}
	else if(xmotor_position>=1885 && xmotor_position<=2115 && ymotor_position>=500 /*&& ymotoPos<=1030*/){
		//printf("\nMove Forward");
		moveToLocation = WALK;
		std::cout << "Walking" << std::endl;
	}
	else if(xmotor_position<1885 && ymotor_position>=500 /*&& ymotoPos<=1030*/){
		//printf("\nTurn Right");
		moveToLocation = TURNRIGHT;
		std::cout << "Turning right" << std::endl;
	}
	else if(xmotor_position>2115 && ymotor_position>=500 /*&& ymotoPos<=1030*/){
		//printf("\nTurn Left");
		moveToLocation = TURNLEFT;
		std::cout << "Turning left" << std::endl;
	}
	else if(xmotor_position<1885 && ymotor_position<500){
		//printf("\nStep Right");
		moveToLocation = STEPRIGHT;
		std::cout << "Stepping right" << std::endl;
	}
	else if(xmotor_position>2115 && ymotor_position<500){
		//printf("\nStep Left");
		moveToLocation = STEPLEFT;
		std::cout << "Stepping left" << std::endl;
	}

	return moveToLocation;
}

//Detect the largest red objects location in the webcam view
int Vision::detectContours(IplImage* img){
	int largest_area=90;
	int largest_contour_index=0;
	int moveBody = 0;
	Rect boundRect;

	IplImage* imgHSV = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
	cvCvtColor(img, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV

	// Get Image Thresh
	//Set the thershold color (ball color) for image tracking
	IplImage* imgThresh = cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
	cvInRangeS(imgHSV, cvScalar(0,137,150), cvScalar(26,256,241), imgThresh);

	cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN,9,9);

	//CvSeq* result;   //hold sequence of points of a contour
	CvMemStorage *storage = cvCreateMemStorage(0); //storage area for all contours

	Mat imgThreshMat(imgThresh);
	Mat imgMat(img);
	vector<vector <Point> > contours; // Vector for storing contour
	vector<Vec4i> hierarchy;
	findContours(imgThreshMat, contours, hierarchy,CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ); // Find the contours in the image

	for( int i = 0; i< contours.size(); i++ ) // iterate through each contour.
	{
		double a=contourArea( contours[i],false);  //Find the area of contour

		if(a>largest_area){
			largest_area=a; //Reset largest area
			largest_contour_index=i;  //Store the index of largest contour
			boundRect=boundingRect(contours[i]); // Find the bounding rectangle for biggest contour

			// For drawing image
			drawContours(imgMat, contours, i, Scalar(255,255,255), CV_FILLED, 8, hierarchy);
		}

	}

	Point centerRec = Point((boundRect.x + boundRect.width/2), (boundRect.y + boundRect.height/2));
	cvCircle(img, cvPoint(centerRec.x,centerRec.y), 4, CV_RGB(255,0,0), -1, 8, 0 );

	Scalar color(255,255,255);
	drawContours( imgThreshMat, contours,largest_contour_index, color, CV_FILLED, 1, hierarchy ); // Draw the largest contour using previously stored index.

	// TODO For showing image
//	imshow("imgThreshMat", imgThreshMat);
//	rectangle(imgMat, boundRect, Scalar(255,255,255), 3, 8, 0);

	//Find what grid the ball is in
	int gridPos = findPos(img, cvRound(centerRec.x),cvRound(centerRec.y));
	//if the ball is not in grid 5 (center) move head to grid 5
	if(gridPos!=0){moveHeadMotors(gridPos);}

	//	cvClearMemStorage(storage);

	cvReleaseMemStorage(&storage);
	//cvReleaseImage(&img);
	cvReleaseImage(&imgThresh);
	cvReleaseImage(&imgHSV);


	//return the grid position the ball is in to main.
	return getBodyMovement();
}

//initialize the Dynamixel USB
void Vision::initMotorUSB(){
	int baudnum = 1;
	int deviceIndex = 0;

	///////// Open USB2Dynamixel ////////////
	if(dxl_initialize(deviceIndex, baudnum) == 0 )
	{
		printf( "Failed to open USB2Dynamixel!\n" );
		printf( "Press Enter key to terminate...\n" );
		getchar();
	}
	else
		printf("Succeed to open USB2Dynamixel!\n");

	return;
}

/*Set the motor numbers
int Vision::setMotor(char motorAxis){
	int motor;
	std::cout<< "Select " << motorAxis << "-motor"<< std::endl;
	std::cin >> motor;
	return motor;
}
 */

//Set the speed of the motors
int Vision::setSpeed(){
	int speed;
	printf("Select motor speed (Between 1-70)");
	std::cin >> speed;
	return speed;
}

int Vision::setCamera() {
	int camera;
	printf("Select camera number:");
	std::cin >> camera;
	return camera;
}

// TODO For drawing image
void Vision::DrawLine(Mat img, Point start, Point end)
{
	int thickness = 2;
	int lineType = 8;
	line( img,
			start,
			end,
			CV_RGB(250,0,0),
			thickness,
			lineType );
}
// TODO For drawing image
void Vision::DrawGridFull(IplImage* img){
	//Draw grid on image
	/*
	 * Vertical lines
	 */
	//Left Side
	Point ptVFTLeft = Point(0,0);
	Point ptVFBLeft = Point(0,500);
	DrawLine(img,ptVFTLeft,ptVFBLeft);
	//Left-Middle
	Point ptVMTLeft = Point(213,0);
	Point ptVMBLeft = Point(213,500);
	DrawLine(img,ptVMTLeft,ptVMBLeft);
	//Right-Middle
	Point ptVMTRight = Point(426,0);
	Point ptVMBRight = Point(426,500);
	DrawLine(img,ptVMTRight,ptVMBRight);
	//Right Side
	Point ptVFTRight = Point(639,0);
	Point ptVFBRight = Point(639,500);
	DrawLine(img,ptVFTRight,ptVFBRight);
	/*
	 * Horizontal lines
	 */
	//Top Side
	Point ptHTLeft = Point(0,0);
	Point ptHTRight = Point(639,0);
	DrawLine(img,ptHTLeft,ptHTRight);
	//Top-Middle
	Point ptHMTLeft = Point(0,160);
	Point ptHMTRight = Point(639,160);
	DrawLine(img,ptHMTLeft,ptHMTRight);
	//Bottom-Middle
	Point ptHMBLeft = Point(0,320);
	Point ptHMBRight = Point(639,320);
	DrawLine(img,ptHMBLeft,ptHMBRight);
	//Bottom Side
	Point ptHBLeft = Point(0,479);
	Point ptHBRight = Point(639,479);
	DrawLine(img,ptHBLeft,ptHBRight);
}
